#include "Creature.h"



void Creature::addMass(const v2& _Pos, const float _Mass)
{
	_masses.push_back(EvoMass(_Pos, _Mass));
}

void Creature::addSpring(const int _M0, const int _M1, const float _Strength, const float _Dampening)
{
	const float length = (_masses[_M1]._pos - _masses[_M0]._pos).len();
	_springs.push_back(EvoSpring(_M0, _M1, length, _Strength, _Dampening));
}

void Creature::update()
{
	for (size_t i = 0; i<_springs.size(); ++i)
	{
		EvoSpring& s = _springs[i];

		// *** muscle flexing ***
		s._extension = 1.f + sinf(SIMULATION_TIME*s._frequency + s._phase)*s._amplitude;

		// *** physics ***
		EvoMass& m0 = _masses[s._masses[0]];
		EvoMass& m1 = _masses[s._masses[1]];

		v2 separation = m1._pos - m0._pos;
		s._current_length = separation.normalise();
		const float force = (s._rest_length*s._extension - s._current_length)*s._strength;

		separation *= force;

		m0._pos -= separation;
		m1._pos += separation;
	}
	for (size_t i = 0; i<_masses.size(); ++i)
	{
		EvoMass& m = _masses[i];
		m._pos[1] += GRAVITY;

		// *** verlet integration ***
		v2 vel = m._pos - m._old_pos;
		m._old_pos = m._pos;
		m._pos += vel*GLOBAL_MASS_DAMPING;

		// *** contrain points to screen ***
		if (m._pos[0]<0) m._pos[0] = 0;
		if (m._pos[0]>WINDOW_WIDTH) m._pos[0] = WINDOW_WIDTH;
		if (m._pos[1]>WINDOW_HEIGHT) m._pos[1] = WINDOW_HEIGHT;
	}
}

void Creature::draw()
{
	glLineWidth(1.f);
	glColor3f(0, 1, 0);
	glBegin(GL_LINES);
	for (size_t i = 0; i<_springs.size(); ++i)
	{
		EvoSpring& s = _springs[i];
		glVertex2fv(_masses[s._masses[0]]._pos.ptr());
		glVertex2fv(_masses[s._masses[1]]._pos.ptr());
	}
	glEnd();

	glPointSize(3.f);
	glColor3f(0, 1, 0);
	glBegin(GL_POINTS);
	for (size_t i = 0; i<_masses.size(); ++i)
	{
		glVertex2fv(_masses[i]._pos.ptr());
	}
	glEnd();
}